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Abstract — During the last decade, Incremental sampling-based 
motion planning algorithms, such as the Rapidly-exploring Ran- 
dom Trees (RRTs) have been shown to work well In practice 
and to possess theoretical guarantees such as probabilistic com- 
pleteness. However, no theoretical bounds on the quality of the 
solution obtained by these algorithms have been established so 
far. The first contribution of this paper is a negative result: it 
is proven that, under mild technical conditions, the cost of the 
best path In the RRT converges almost surely to a non-optimal 
value. Second, a new algorithm Is considered, called the Rapidly- 
exploring Random Graph (RRG), and It is shown that the cost of 
the best path in the RRG converges to the optimum almost surely. 
Third, a tree version of RRG is Introduced, called the RRT* 
algorithm, which preserves the asymptotic optlmality of RRG 
while maintaining a tree structure like RRT. The analysis of the 
new algorithms hinges on novel connections between sampling- 
based motion planning algorithms and the theory of random 
geometric graphs. In terms of computational complexity, it is 
shown that the number of simple operations required by both the 
RRG and RRT* algorithms is asymptotically within a constant 
factor of that required by RRT. 

I. Introduction 

The robotic motion planning problem has received a con- 
siderable amount of attention, especially over the last decade, 
as robots started becoming a vital part of modern industry as 
well as our daily life. Even though modern robots may possess 
significant differences in sensing, actuation, size, workspace, 
application, etc., the problem of navigating through a complex 
environment is embedded and essential in almost all robotics 
applications. Moreover, this problem has several applications 
in other disciplines such as verification, computational biology, 
and computer animation [l]-[6]. 

Informally speaking, given a robot with a description of its 
dynamics, a description of the environment, an initial state, and 
a set of goal states, the motion planning problem is to find a 
sequence of control inputs so as the drive the robot from its 
initial state to one of the goal states while obeying the rules 
of the environment, e.g., not colliding with the surrounding 
obstacles. An algorithm that solves this problem is said to be 
complete if it returns a solution when one exists and returns 
failure otherwise. 

Early algorithmic approaches to the motion planning prob- 
lem mainly focused on developing complete planners for, e.g., 
polygonal robots moving among polygonal obstacles [7], and 
later for more general cases using algebraic techniques [8]. 
These algorithms, however, suffered severely from compu- 
tational complexity, which prohibited their practical imple- 
mentations; for instance, the algorithm in [8] had doubly 
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exponential time complexity. Regarding the computational 
complexity of the problem, a remarkable result was proven 
as early as 1979: Reif showed that a most basic version of the 
motion planning problem, called the piano movers problem, 
was PSPACE-hard [9], which strongly suggested that complete 
planners are would be unsuitable for practical applications. 

Practical planners came around with the development of 
cell decomposition methods [10], [11], potential fields [12], 
and roadmap methods [13]. These approaches relaxed the 
completeness requirement to, for instance, resolution com- 
pleteness, which guarantees completeness only when the res- 
olution parameter of the algorithm is set fine enough. These 
planners demonstrated remarkable performance in accomplish- 
ing various tasks in complex environments within reasonable 
time bounds [14]. However, their practical applications were 
mostly limited to state spaces with up to five dimensions, since 
decomposition-based methods suffered from large number of 
cells, and potential field methods from local minima [1], [15]. 

Arguably, some of the most influential recent advances 
in robotic motion planning can be found in sampling-based 
algorithms [16]-[18], which attracted a large amount of at- 
tention over the last decade, including very recent work 
(see, e.g., [19]-[25]). In summary, sampling-based algorithms 
connect a set of points that are chosen randomly from the 
obstacle-free space in order to build a roadmap, which is 
used to form a strong hypothesis of the connectivity of the 
environment, in particular the connectivitiy of the initial state 
and the set of goal states. Informally speaking, sampling-based 
methods provide large amounts of computational savings by 
avoiding explicit construction of obstacles in the state space, 
as opposed to most complete motion planning algorithms. 
Even though these algorithms do not achieve completeness, 
they provide probabilistic completeness guarantees in the sense 
that the probability that the planner fails to return a solution, 
if one exists, decays to zero as the number of samples 
approaches infinity [26]. Moreover, the rate of decay of the 
probability of failure is exponential, under the assumption that 
the environment has good "visibility" properties [26]. More 
recently, the empirical success of sampling-based algorithms 
was argued to be strongly tied to the hypothesis that most 
practical robotic applications, even though involving robots 
with many degrees of freedom, feature environments with such 
good visibility properties [27]. 

A. Sampling-Based Algorithms 

In practical applications, the class of sampling-based mo- 
tion planning algorithms, including, for instance, Probabilistic 
Roadmaps (PRMs) [17], [28], and Rapidly-exploring Random 



Trees (RRTs) [18], [29], is one of the most widely-used 
approaches to the motion planning problem [30], [31]. Even 
though the idea of connecting samples from the state space 
is essential in both approaches, these two algorithms mainly 
differ in the way that they construct the roadmap. 

The PRM algorithm and its variants are multiple-query 
methods that first construct a graph (the roadmap), which 
represents a large portion of the collision-free trajectories, 
and then answer queries by computing a shortest path that 
connects the initial state with a final state through the graph. 
Shortly after their introduction in the literature, PRMs have 
been reported to extend to high-dimensional state spaces [17], 
since they avoid explicit construction of obstacles in the state 
space of the robot. Subsequently, it was shown that the PRM 
algorithm is probabilistically complete and the probability of 
failure decays to zero exponentially [28]. During the last 
decade, the PRM algorithm has been a focus of robotics 
research. In particular, several improvements were suggested 
by many authors and the reasons to why it performs well in 
several practical cases were better understood (see, e.g., [27], 
[32], [33] for some examples). 

Even though multiple-query methods are valuable in highly 
structured environments such as factory floors, most online 
planning problems do not require multiple queries, since, for 
instance, the robot moves from one environment to another, or 
the environment is not known a priori. Moreover, in some ap- 
plications, computing a roadmap a priori may be computation- 
ally challenging or even infeasible. Tailored mainly for these 
applications, incremental sampling-based planning algorithms 
have emerged as an online counterparts to the PRMs (see, 
e.g., [29], [34]). The incremental nature of these algorithms 
allows termination as soon as a solution is found. Furthermore, 
slight changes in the environment do not require planning from 
scratch, since most of the trajectories in the tree are still feasi- 
ble (this is a property shared with most other roadmap-based 
methods) or can be improved to produce feasible trajectories 
quickly. These properties provide significant computational 
savings making online real-time implementations possible. 

A widely-studied incremental algorithm is the RRT. Closely 
after its introduction in the literature, several theoretical guar- 
antees provided by the RRT algorithm were shown, including 
probabilistic completeness [29] and exponential rate of decay 
of the probability of failure [35]. Subsequently, the RRT 
algorithm has been improved and found many applications 
in the robotics domain and elsewhere (see for instance [2]- 
[4], [35]-[37]). In particular, RRTs have been shown to work 
effectively for systems with differential constraints, nonlinear 
dynamics, and non-holonomic constraints [18], [35] as well 
as purely discrete or hybrid systems [36]. Moreover, the 
RRT algorithm was demonstrated in major robotics events on 
various experimental robotic platforms [38]-[42]. 

An interesting extension of the RRT that is worth men- 
tioning at this point is the Rapidly-exploring Random Graph 
(RRG) algorithm [43]. The RRG algorithm was proposed as an 
incremental sampling-based method in order to extend RRTs 
to generate feasible trajectories for satisfying specifications 
other than the usual "avoid obstacles and reach the goal". 
In [43], the authors have shown that the RRG algorithm 



can handle any specification given in the form of /i-calculus, 
which includes the widely-used specification language Linear 
Temporal Logic (LTL) as a strict subclass. The RRG algorithm 
incrementally builds a graph, instead of a tree, since several 
specifications in LTL (e.g., w-regular properties [44]) require 
cyclic trajectories, which are not included in trees. 



B. Optimal Motion Planning 

Generally, the standard robotic motion planning problem 
does not take the "quality" of the solution into account. That is, 
the focus of the problem is to find a trajectory that starts from 
the initial state and reaches one of the goal states while staying 
inside the obstacle-free portion of the state space. Let us note 
that, as mentioned earlier, even this form of the problem is 
computationally challenging [9]. Consequently, there has been 
a significant amount of research effort devoted to construct- 
ing feasible solutions in a computationally effective manner 
Hence, most of the existing algorithms pay almost no attention 
to the other properties of solution, e.g., its length, or its cost. 
As a result, the analysis of the quality of the solution returned 
by the algorithm, or algorithms tailored to provably improve 
the quality of the solution if kept running, have received 
relatively little attention, even though their importance is 
mentioned in early seminal papers [18]. In fact, in many field 
implementations of such algorithms (see, e.g., [39]), it is often 
the case that a feasible path is found quickly, and additional 
computation time is devoted to improving the solution with 
heuristics until the solution is executed. 

Yet, the importance of the quality of the solution returned by 
the planners have been noticed, in particular, from the point of 
view of incremental sampling-based motion planning. In [45], 
Urmson and Simmons have proposed heuristics to bias the 
tree growth in the RRT towards those regions that result in 
low-cost solutions. They have also shown experimental results 
evaluating the performance of different heuristics in terms of 
the quality of the solution returned. In [46], Ferguson and 
Stentz have considered running the RRT algorithm multiple 
times in order to progressively improve the quality of the 
solution. They showed that each run of the algorithm results 
in a path with smaller cost, even though the procedure is not 
guaranteed to converge to an optimum solution. (Criteria for 
restarting multiple RRT runs, in a different context, were also 
proposed in [47].) 

A different approach that also offers optimality guarantees 
is based on graph search algorithms (such as A*) applied over 
a grid that is generated offline. Recently, these algorithms re- 
ceived a large amount of research attention. In particular, they 
were extended to run in an anytime fashion [48], [49], deal 
with dynamic environments [49], [50], and handle systems 
with differential constraints [51]. The have also been success- 
fully demonstrated on various robotic platforms [51], [52]. 
However, optimality guarantees of these algorithms are only 
ensured up to the grid resolution. Moreover, since the number 
of grid points grows exponentially with the dimensionality of 
the state-space, so does the (worst-case) running time of these 
algorithms. 



C. Statement of Contributions 

To the best of our knowledge, this paper provides the 
first thorough analysis of optimality properties of incremental 
sampling-based motion planning algorithms. In particular, we 
show that the probability that the RRT converges to an opti- 
mum solution, as the number of samples approaches infinity, is 
zero under some reasonable technical assumptions. In fact, we 
give a formal proof of the fact that the RRT algorithm almost 
always converges to a suboptimal solution. Second, we show 
that the probability of the same event for the RRG algorithm 
is one. That is, the RRG algorithm is asymptotically optimal 
in the sense that it converges to an optimal solution almost 
surely as the number of samples approaches infinity. Third, we 
propose a novel variant of the RRG algorithm, called RRT*, 
which inherits the asymptotic optimality of the RRG algorithm 
while maintaining a tree structure. To do so, the RRT* algo- 
rithm essentially "rewires" the tree structure as it discovers 
new lower-cost paths reaching the nodes that are already in 
the tree. Finally, we show that the asymptotic computational 
complexity of the RRG and RRT* algorithms is essentially the 
same as that of RRTs. We also provide experimental evidence 
supporting our theoretical computational complexity results. 
We discuss the relation between the proposed algorithms and 
PRM-based methods, and propose a PRM-like algorithm that 
improves the efficiency of PRM while maintaining the same 
feasibility and optimality properties. 

To our knowledge, the algorithms considered in this paper 
are the first computationally efficient incremental sampling- 
based motion planning algorithms with asymptotic optimality 
guarantees. Indeed, our results imply that these algorithms are 
optimal also from an asymptotic computational complexity 
point of view, since they closely match lower bounds for 
computing nearest neighbors. The key insight is that connec- 
tions between vertices in the graph should be sought within 
balls whose radius vanishes with a certain rate as the size 
of the graph increases, and is based on new connections 
between motion planning and the theory of random geometric 
graphs [53], [54]. 

In this paper, we consider the problem of navigating through 
a connected bounded subset of a d-dimensional Euclidean 
space. As in the early seminal papers on incremental sampling- 
based motion planning algorithms such as [29], we assume 
no differential constraints, but our methods can be easily 
extended to planning in configuration spaces and applied to 
several practical problems of interest. We defer the extension 
to systems with differential constraints to another paper. 

D. Paper Organization 

This paper is organized as follows. In Section II lays 
the ground in terms of notation and problem formulation. 
Section III is devoted to the introduction of the RRT and 
RRG algorithms. In Section IV these algorithms are analyzed 
in terms of probabilistic completeness, asymptotic optimality, 
and computational complexity. The RRT* algorithm is pre- 
sented in Section V where it is shown that RRT* inherits 
the theoretical guarantees of the RRG algorithm. Experimental 
results are presented and discussed in Section VI Concluding 



remarks are given in Section VII In order not to disrupt 
the flow of the presentation, proofs of important results are 
presented in the Appendix. 



II. Preliminary Material 



A. Notation 



Let N denote the set of non-negative integers and M denote 
the set of reals. Let M>o and M>o denote the sets of positive 
and non-negative reals, respectively. The set N>o is defined 
similarly. A sequence on a set A is a mapping from N to 
A, denoted as {aijigpf, where a^ e A is the element that 
i e N is mapped to. Given a, & G M, closed and open intervals 
between a and h are denoted by [a, b] and (a, h), respectively. 
More generally, if a, 6 G W^, then [a, h] is used to denote the 
line segment that connects a with h, i.e., [a, b] := {ra + (1 — 
t)&|t € M, < T < 1}. The Euclidean norm is denoted by 
II • II . Given a set X C K'', the closure of X is denoted by 
c\{X). The closed ball of radius r > centered at x e M'' 
is defined as Bx.r '■— {y G M"^ | ||y — a;|| < r}; Bx,r is also 
called the r— ball centered at x. Given a set X C M'', the 
Lebesgue measure of X, i.e., its volume, is denoted by IJ.(X). 
The volume of the unit ball in R'^ is denoted by (d- 

Given a set X C K'', and a scalar s > 0, a path 
in X is a continuous function a : [0,s] -^ X, where 
s is the length of the path defined in the usual way as 

sup{„gN,o=To<ri<-<r„=s}ELilk(^0 " '^('^^-i)!!- Given 
two paths in X, cti : [0, si] -^ X, and <T2 : [0, S2] -^ X, 
with CTi(si) — CT2(0), their concatenation is denoted by <ti\(T2- 
More precisely, a — a\\a2 : [0, si +52]^-^^ is defined as 
cr(s) = o-i(s) for all s G [0, Si], and 17(5) = 0-2(5 — si) for all 
s e [si, si + S2]. More generally, a concatenation of multiple 
paths cri|cr2| •• • |o-„ is defined as (• • • ((cri|cr2)|cr3)| • • • |cr„). 
The set of all paths in X with nonzero length is denoted 
by Sx- The straight continuous path between two points 
x\,X2 € K'' is denoted by Line(a;i,X2). 

Given a probability space (il, J^, P), where il is a sample 
space, T C 2^^ is a ct— algebra, and P is a probability measure, 
an event A is an element of T . The complement of an event 
A is denoted by A'^ . Given a sequence of events {AijigN, the 
event n°^]^ U^ Ai is denoted by limsupj_j.^ Ai (also called 
the event that Ai occurs infinitely often). A (real) random 
variable is a measurable function that maps VL into M. An 
extended (real) random variable can also take the values ±00. 
A sequence of random variables {3^i}igN is said to converge 
surely to a random variable y if lim^^oo 3^i(^) — y(y>) 
for all a; e ri; the sequence is said to converge almost 
surely if P({limi_).oo 3^i = 3^}) = 1- Finally, if (^(w) is 
a property that is either true or false for a given a; e fJ, 
the event that denotes the set of all samples uj for which 
^piuj) holds, i.e., {cj e fJ | (^(w) holds}, is written as {</?}, 
e.g., {cj G 17 I Xva\i^rx,yi(yj) = 3^('^)} is simply written as 

{lim.^oo y^ = y}- 

Let f{n) and g{n) be two functions with domain and range 
N. The function /(n) is said to be 0(g{n)), denoted as f{n) G 
0(g{n)), if there exists two constants M and uq such that 
fin) < Mg{n) for all n > uq- The function /(n) is said 
to be il{g{n)), denoted as f{n) G il{g{n)), if there exists 



constants M and rig such that f{n) > Mg{n) for all n > hq. 
The function f{n) is said to be Q{g{n)), denoted as f{n) E 
e(g(n)), if f{n) G 0{g{n)) and /(n) e r!(g(n)). 

Let X be a subset of R'^. A (directed) graph G = (V, i?) on 
X is composed of a vertex set F and an edge set E, such that 
F is a finite subset of X, and E is a subset of V x V^. The 
set of all directed graphs on X is denoted by Qx- A directed 
path on G is a sequence {vi,V2, ■ ■ ■ , ««) of vertices such that 
{vi, Wi+i) e i? for all 1 < 2 < n — 1. Given a vertex v E V, 
the sets {u e F | (u, w) e i?} and {u e V\{v,u) e i<;} 
are said to be its incoming neighbors and outgoing neighbors, 
respectively. A (directed) tree is a directed graph, in which 
each vertex but one has a unique incoming neighbor; the vertex 
with no incoming neighbor is called the root vertex. Vertices 
of a tree are often also called nodes. 

B. Problem Formulation 

In this section, two variants of the path planning problem 
are presented. First, the feasibility problem in path planning 
is formalized, then the optimality problem is introduced. 

Let X be a bounded connected open subset of W'-, where 
d G N, d > 2. Let Xobs and Xgoai, called the obstacle region 
and the goal region, respectively, be open subsets of X. Let 
us denote the obstacle-free space, i.e., X\Xobs, as X^^cc- Let 
the initial state, Xinit, be an element of X^ee- In the sequel, a 
path in Xfree is said to be a collision-free path. A collision-free 
path that starts at a:;init and ends in the goal region is said to 
be a. feasible path, i.e., a collision-free path a : [0, s\ -^ ^froc 
is feasible if and only if cr(0) — Xinit and cr{s) G Xgos\. 

The. feasibility problem of path planning is to find a feasible 
path, if one exists, and report failure otherwise. The problem 
can be formalized as follows. 

Problem 1 (Feasible planning) Given a bounded connected 
open set X C W^, an obstacle space Xobs C X, an initial 
state Xinit G Xfroc, and a goal region Xgoai C ^froc, find a 
path a : [0, s] -^ ^froc such that cr(0) — xinit and a{s) G 
Xgoai, if one exists. If no such path exists, then report failure. 

Let c : Sxffe„ -^ R>o be a function, called the cost 
function, which assigns a non-negative cost to all nontrivial 
collision-free paths. The optimality problem of path planning 
asks for finding a feasible path with minimal cost, formalized 
as follows. 

Problem 2 (Optimal planning) Given a bounded connected 
open set X, an obstacle space Xobs, on initial state Xi^n, and 
a goal region Xgoai, find a path a* : [0, s] — > cl(Xfi.oc) such 
that (i) cr*(0) = a:;init and cr*(s) G ^goai, and (ii) c{a*) = 
mincres^,,^ c((t). If no such path exists, then report failure. 

III. Algorithms 

In this section, two incremental sampling-based motion 
planning algorithms, namely the RRT and the RRG algorithms, 
are introduced. Before formalizing the algorithms, let us note 
the primitive procedures that they rely on. 

Sampling: The function Sample : N -^ -'^frco returns 
independent identically distributed (i.i.d.) samples from Xfrcc- 



Steering: Given two points x,y E X, the function Steer : 
{x, y) 1-^ z returns a point z G M'' such that z is "closer" to y 
than X is. Throughout the paper, the point z returned by the 
function Steer will be such that z minimizes ||z — y\\ while 
at the same time maintaining ||z — a;|| < ry, for a prespecified 
?7 > 0, ' i.e.. 



Steer(a;,y) = argmin^gRd n 



ii<r,F-y|| 



Nearest Neighbor: Given a graph G — (V, E) G Gxf^^^ 
and a point x G Xfrco , the function Nearest : (G, x) i-^ v 
returns a vertex v E V that is "closest" to x in terms of a 
given distance function. In this paper, we will use Euclidean 
distance (see, e.g., [18] for alternative choices), i.e., 

Nearest(G = (V, E), x) — argiiiin^gy||a; — v\\. 

Near Vertices: Given a graph G = {V,E) G Gxh^^, a 
point X G ^froc, and a number n G N, the function Near : 
(G, X, n) i--> V' returns a set V' of vertices such that V' C V. 
The Near procedure can be thought of as a generalization of 
the nearest neighbor procedure in the sense that the former 
returns a collection of vertices that are close to x, whereas the 
latter returns only one such vertex that is the closest. Just like 
the Nearest procedure, there are many ways to define the 
Near procedure, each of which leads to different algorithmic 
properties. For technical reasons to become clear later, we 
define Near(G,a;,n) to be the set of all vertices within the 
closed ball of radius r„ centered at x, where 

and 7 is a constant. Hence, the volume of this ball is 

r log n > c/t 

mm{7^,Q'7 }■ 

Collision Test: Given two points x,x' E Xfrcc, the Boolean 
function ObstacleFree(a;,a;') returns True iff the line seg- 
ment between x and x' lies in Xfroo, i-C-, [x, x'] C ^free- 
Both the RRT and the RRG algorithms are similar to most 
other incremental sampling-based planning algorithms (see 
Algorithm 1). Initially, the algorithms start with the graph that 
includes the initial state as its single vertex and no edges; then, 
they incrementally grow a graph on X^eo by sampling a state 
a^rand from Xfroc at random and extending the graph towards 
a^rand- In the sequel, every such step of sampling followed 
by extensions (Lines 2 5 of Algorithm 1) is called a single 
iteration of the incremental sampling-based algorithm. 

Hence, the body of both algorithms, given in Algorithm 1 is 
the same. However, RRGs and RRTs differ in the choice of the 
vertices to be extended. The Extend procedures for the RRT 
and the RRG algorithms are provided in Algorithms 2 and 3 
respectively. Informally speaking, the RRT algorithm extends 
the nearest vertex towards the sample. The RRG algorithm first 
extends the nearest vertex, and if such extension is successful, 
it also extends all vertices returned by the Near procedure. In 
both cases, all extensions resulting in collision-free trajectories 

'This steering procedure is used widely in tlie robotics literature, since its 
introduction in [29]. Our results also extend to the Rapidly-exploring Random 
Dense Trees (see, e.g., [30]), which are slightly modified versions of the RRTs 
that do not require tuning any prespecified parameters such as rj in this case. 



are added to the graph as edges, and their terminal points as 
new vertices. 



Algorithm 1: Body ot RRT and RRG Algorithms 


1 V ^ {a^init}; E^%-i^Q- 


2 while i < A^ do 


3 


G^ {V,E); 


4 


strand ^ Sample(i); i <— i + 1; 


5 


_ {V,E) ^Extend(G,a;rand); 



Algorithm 2: Extendjj^jj^T 



1 V ^V; E' ^ E; 

2 a;ncarost ^ Nearest (G, x); 

3 -^ncw ^ ^teert^Xncarcst 7 -^j? 

4 if DbstacleFree(xnoarest,a;n 

5 r ^V'U {a;„ew}; 

6 E ^E U {(a;noarcst7 2^110 

7 return G' = {V , E') 



w) then 

.)}; 



Algorithm 3: Ext end j<^j^^g 



1 y ^ y; £;' ^ S; 

2 ^nearest <" Nearest (G, x); 

3 '^ncw ^ "^teer (^Xncarcst 7 -^j? 

4 if ObstacleFree(xnoarost,x„ow) then 

V ^v'yj {Xnew}; 

-^ "^ -^ U H.'^ncarcst 7 -^ncw j 7 V-^ncwi ^nearest j J? 

^ncar <- Near(G, Xncw, 1^1); 

for all Xnoar G ^noar dO 

if ObstacleFree(xnow, Xjiear) then 



11 return G' = (V,^;') 

Notice that the graph G maintained by the RRT algorithm 
is a tree, whereas that maintained by the RRG can, in general, 
include vertices with more than one incoming neighbor. 

IV. Analysis 
A. Convergence to a Feasible Solution 

In this section, the feasibility problem is considered. It 
is proven that the RRG algorithm inherits the probabilistic 
completeness as well as the exponential decay of the proba- 
bility of failure (as the number of samples increase) from the 
RRT. These results imply that the RRT and RRG algorithms 
have the same performance in producing a solution to the 
feasibility problem as the number of samples increase. Before 
formalizing these claims, let us introduce some notation and 
preliminaries. 

Note that the only randomness in Algorithms 13 may arise 
from the sampling procedure denoted as Semiple^ To model 

^We will not address the case in which the sampling procedure is determin- 
istic, but refer the reader to [55], which contains an in-depth discussion of 
the relative merits of randomness and determinism in sampling-based motion 
planning algorithms. 



this randomness, we define the sample space Vl to be the 
infinite cartesian product X^^^. Intuitively, a single element lo 
of Q, denotes a sequence {Sample (i)}i£pj of infinitely many 
samples drawn from X^^cc, and Q, denotes the set of all such 
sequences. 

Sets of vertices and edges of the graphs maintained by 
the RRT and the RRG algorithms, then, can be defined 
as functions from the sample space il to appropriate sets. 
More precisely, let {Vj^^'^jigN and {V^^^'^jigN, sequences of 
functions defined from Vt into finite subsets of Xfj-co, be the 
sets of vertices in the RRT and the RRG, respectively, at the 

RRT _ vRRG _ 



V, 







end of iteration i. By convention, we define Vg 
{xinit}. Similarly, let £f^^ and Sf^'^, defined for all i e N, 
denote the set of edges in the RRT and the RRG, respectively, 
at the end of iteration i. Clearly, £^^^ ^ Sf^'^ == 0. 

An important lemma used for proving the equivalency 
between the RRT and the RRG algorithms is the following. 



Lemma 3 For all i (z N and all 

V^^'^iiu) and £f «T(^) c Ef^^iio). 



e n, Vf'^i(co) 



Lemma 3 implies that the paths discovered by the RRT 
algorithm by the end of iteration i is, essentially, a subset of 
those discovered by the RRG by the end of the same iteration. 

An algorithm addressing Problem 1 is said to be proba- 
bilistically complete if it finds a feasible path with probability 
approaching one as the number of iterations approaches infin- 
ity. Note that there exists a collision-free path starting from 
Xinit to any vertex in the tree maintained by the RRT, since 
the RRT maintains a connected graph on Xfrco that necessarily 
includes Xinit- Using this fact, the probabilistic completeness 
property of the RRT is stated alternatively as follows. 

Theorem 4 (see [18]) If there exists a feasible solution to 

Problem 1 then lim.^oo P {{Vf^^ H Xgoai 7^ 0}) == L 

Moreover, under certain assumptions on the environment, 
the probability that the RRT fails to find a feasible path, even 
though one exists, decays to zero exponentially (see, e.g., 
[18], [34]). We state these assumptions here and then state 
the theorem in our notation. 

An attraction sequence [18] is defined as a finite sequence 
A = {^1,^2, . . . 7 Afc} of sets as follows: (i) A^ — {xjuit}, 
and (ii) for each set Ai, there exists a set Bi, called the basin 
such that for any x E Ai^i, y E Ai, and z E X \ Bi, there 
holds ||x — j/ll < ||a; — z|p Given an attraction sequence A of 

length k, let pk denote inm,^[i^2,...,k} [ J{xl^l) )- 

The following theorem states that the probability that the 
RRT algorithm fails to return a solution, when one exists, 
decays to zero exponentially fast. 

Theorem 5 (see [18]) If there exists an attraction sequence 

A of length k, then P {{Vf^^ C^ Xgoai == 0}) < e-^'^^P^-^^\ 

As noted in [18], the convergence gets faster as the length 
of the attraction sequence gets smaller and the volume of 

^ Since we do not consider any differential constraints, some of the other 
assumptions introduced in [18] are immediately satisfied. 



the minimum volume set in the attraction sequence gets 
larger. Such properties are achieved when the environment 
does not involve narrow passages and, essentially, has good 
"visibility" properties. (See, e.g., [27] for dependence of the 
performance of probabilistic planners such as PRMs on the 
visibility properties of the environment). 

With Lemma 3 and Theorems 4 and 5, the following 
theorem is immediate. 

Theorem 6 If there exists a feasible solution to Prob- 
lem 1, then limj_>ooP({Vf'^^nXgoai 7^0}) = 1. More- 
over, if an attraction sequence A of length k exists, then 
P ({Vf^G p x^,,, = 0}) < e-5(*P'=-2fe). 

B. Asymptotic Optimality 

This section is devoted to the investigation of optimal- 
ity properties of the RRT and the RRG algorithms. First, 
under some mild technical assumptions, we show that the 
probability that the RRT converges to an optimal solution 
is zero. However, the convergence of this random variable is 
guaranteed, which implies that the RRT converges to a non- 
optimum solution with probability one. On the contrary, we 
subsequently show that the RRG algorithm converges to an 
optimum solution almost-surely. 

Let iX^^^jigN be a sequence of extended random variables 
that denote the cost of a minimum-cost path contained within 
the tree maintained by the RRT algorithm at the end of 
iteration i. The extended random variable yf-^'-' is defined 
similarly. Let c* denote the cost of a minimum-cost path in 
cl(Xfroo), i.e., the cost of a path that solves Problem 2 

Let us note that the limits of these two extended random 
variable sequences as i approaches infinity exist. More for- 
mally, notice that y^i^{uj) < y^^^{iu) holds for all i e N 
and all uj £ il. Moreover, we have that yf^^{uj) > c* for all 
i E N and all oj G O, by optimality of c*. Hence, {J^j^^^'^ligN 
is a surely non-increasing sequence of random variables that 
is surely lower-bounded by c*. Thus, for all ui E il, the limit 
hnii^ca yf^"^ {^) exists. The same argument also holds for 
the sequence {J^j^^'^jigfj. 

1) Almost Sure Suboptimality of the RRT: Let us note the 
following assumptions, which will be required to show the 
almost-sure sub-optimality of the RRT. Let S* denote the 
set of all optimal paths, i.e., the set of all paths that solve 
Problem 2, and Xopt denote the set of states that an optimal 
path in S* passes through, i.e.. 



Assumption 9 (Monotonicity of the Cost Function) For 

all (Ti,cr2 G '^Xfraa' f^ cost function c satisfies the following: 

c(cri) < c{ai\a2). 

Assumptions 7 rules out trivial cases, in which the RRT 
algorithm can sample exactly an optimal path with non-zero 
probability. Note that this assumption is placed on the problem 
instance rather than the algorithm. Most cost functions and 
problem instances of interest satisfy this assumption, includ- 
ing, e.g., the Euclidean length of the path. Let us note that this 
assumption does not imply that there is a single optimal path; 
indeed, there are problem instances with uncountably many 
optimal paths, for which Assumption 7 holds. Assumption 8 
also ensures that the sampling procedure can not be tuned 
to construct the optimal path exactly. Finally, Assumption 9 
merely states that extending a path to produce a longer path 
can not decrease its cost. 

Recall that d denotes the dimensionality of the state space. 
The negative result of this section is formalized as follows. 

Theorem 10 Let Assumptions 7. 8 and 9 hold. Then, the 
probability that the cost of the minimum-cost path in the RRT 
converges to the optimal cost is zero, i.e.. 



({l„X"- = c.})=0. 



whenever d > 2. 



As noted before, the limit lim^^oo X^^"'"(w) exists and is 
a random variable. However, Theorem 10 directly implies 
that this limit is strictly greater than c* with probability 
one, i.e., P ({limi^oo yf'^^ > c*}) = 1. In other words, we 
establish, as a corollary, that the RRT algorithm converges to 
a suboptimal solution with probability one. 

Remark 11 (Effectiveness of Muhiple RRTs) Since the 
cost of the best path returned by the RRT algorithm converges 
to a random variable, say y^^. Theorem 10 provides new 
insight explaining the effectiveness of approaches as in [46]. 
In fact, running multiple instances of the RRT algorithm 
amounts to drawing multiple samples ofy^"'". 

2) Almost Sure Optimality of the RRG: Let us note the 
following set of assumptions, which will be required to show 
the asymptotic optimality of the RRG. 

Assumption 12 (Additivity of the Cost Function) For all 

Oi,a2 G Y^Xh^^, the cost function c satisfies the following: 

c{ai\a2) = c(cri) + c{(j2). 



Assumption 7 (Zero-measure Optimal Paths) The set of 

all points in the state-space that an optimal trajectory passes 
through has measure zero, i.e., fj, (^opt) = 0. 

Assumption 8 (Sampling Procedure) The sampling proce- 
dure is such that the samples {Sample(i)}igN are drawn 
from an absolutely continuous distribution with a continuous 
density function f{x) bounded away from zero on Xfj-cc. 



Assumption 13 (Continuity of the Cost Function) The 

cost function c is Lipschitz continuous in the following sense: 
there exists some constant k such that for any two paths 

ai : [0, si] -^ Xfroc and (72 : [0, S2] -> -'^froo, 

|c(cri) -c(o-2)| < K sup ||cri(rsi) - o-2(ts2)||- 

Te[o,i] 

Assumption 14 (Obstacle Spacing) There exists a constant 
S G M+ such that for any point x G Xfroo, there exists x' G 



Xfroo, such that (i) the S-ball centered at x' lies inside Xfrco, 
i.e., Bx'.s C Xfroc, ond (ii) x lies inside the 6-ball centered at 
x' , i.e., X e B^i _i. 

Assumption 13 ensures that two paths that are very close 
to each other have similar costs. Let us note that several cost 
functions of practical interest satisfy Assumptions 12 and 13 
An example that is widely used, e.g., in optimal control, is 
the line integral of a continuous function g : Xfrco ^ ]R>o 
over trajectories of bounded length, i.e., c(cr) = L g(a{T))dT. 
Assumption 14 is a rather technical assumption, which ensures 
existence of some free space around the optimal trajectories 
to allow convergence. We also assume for simplicity that 
the sampling is uniform, although our results can be easily 
extended to more general sampling procedures. 

Recall that d is the dimensionality of the state-space X, 
and 7 is the constant defined in the Near procedure. The 
positive result that states the asymptotic optimality of the RRG 
algorithm can be formalized as follows. 

Theorem 15 Let Assumptions 12, 13 and 14 hold, and as- 
sume that Problem 1 admits a feasible solution. Then, the cost 
of the minimum-cost path in the RRG converges to c* almost 
surely, i.e., 

p({.limJ^fRG-c*})-l, 

whenever d > 2 and 7 > 7l := 2'^(1 + l/d)fi{Xf,-cc). 

C. Computational Complexity 

The objective of this section is to compare the computational 
complexity of RRTs and RRGs. It is shown that these algo- 
rithms share essentially the same asymptotic computational 
complexity in terms of the number of calls to simple oper- 
ations such as comparisons, additions, and multiplications. 
Simple operations should not be confused with primitive 
procedures outlined in Section 111, which are higher level 
functions that may have different asymptotic computational 
complexities in terms of the number of simple operations that 
they perform. Note that the objective of this section is not the 
evaluation of the computational complexity in absolute terms, 
but to compare the relative complexity of the two algorithms, 
hence the input of interest is the number of iterations, as 
opposed to parameters describing the problem instance. 

Let us first investigate the computational complexity of the 
RRT and the RRG algorithms in terms of the number of calls 
to the primitive procedures. Notice that, in every iteration, the 
number of calls to Ssimple, Steer, and Nearest procedures 
are the same in both algorithms. However, number of calls 
to Near and ObstacleFree procedures differ: the former is 
never called by the RRT and is called at most once by the 
RRG, whereas the latter is called exactly once by the RRT 
and at least once by the RRG. Informally speaking, we first 
show that the expected number of calls to the ObstacleFree 
procedure by the RRG algorithm is O(logn), where n is the 
number of vertices in the graph. 

Let Of^'^ be a random variable that denotes the number of 
calls to the ObstacleFree procedure by the RRG algorithm in 
iteration i. Notice that, as an immediate corollary of Lemma 3 



the number of vertices in the RRT and RRG algorithms is the 
same at any given iteration. Let Afi be the number of vertices 
in these algorithms at the end of iteration i. The following 
theorem establishes that the expected number of calls to the 
ObstacleFree procedure in iteration i by the RRG algorithm 
scales logarithmically with the number of vertices in the graph 
as i approaches infinity. 

Lemma 16 In the limit as i approaches infinity, the random 
variable 0^^'~' / \og{Afi) is no more than a constant in expec- 
tation, i.e.. 



lim sup E 



< 



.log(M) 

where (f> € M>o is a constant that depends only on the problem 
instance. 

Hence, informally speaking, we have established that the 
RRG algorithm has an overhead of order logn calls to the 
ObstacleFree procedure and one call to the Near procedure, 
where n is the number of vertices in the RRG; otherwise, both 
algorithms scale the same in terms of number of calls to the 
primitive procedures. 

However, some primitive procedures clearly take more 
computation time to execute than others. For a more fair 
comparison, we also evaluate the time required to execute each 
primitive procedure in terms of the number of simple opera- 
tions (also called steps in this paper) that they perform. This 
analysis shows that the expected number of simple operations 
performed by the RRG is asymptotically within a constant 
factor of that performed by the RRT, which establishes that 
the RRT and the RRG algorithms have the same asymptotic 
computational complexity in terms of number of calls to the 
simple operations. 

First, notice that Sample, Steer, and ObstacleFree pro- 
cedures can be performed in a constant number of steps, i.e., 
independent of the number of vertices in the graph. (Although 
all these procedures clearly depend on parameters such as the 
dimensionality of the state-space and the number of obstacles, 
these are fixed for each problem instance.) 

Second, let us consider the computational complexity of 
the Nearest procedure. Indeed, the nearest neighbor search 
problem has been widely studied in the literature, since it 
has many applications in, e.g., computer graphics, database 
systems, image processing, data mining, patern recognition, 
etc. [56], [57]. Let us assume that the distance computation 
between two points can be done in 0{d) time (note that for the 
purposes of comparing distances, the Euclidean distance can 
be evaluated without computing any roots). Clearly, a brute- 
force algorithm that examines every vertex runs in 0{dn) 
time and requires 0(1) space. However, in many online real- 
time applications such as robotics, it is highly desirable to 
reduce the computation time of each iteration under sublinear 
bounds, especially for anytime algorithms, which provide 
better solutions as the number of iterations increase. 

Fortunately, it is possible to design nearest neighbor com- 
putation algorithms that run in sublinear time. Generally, 
such efficient computations of nearest neighbors are based 
on constructing a tree structure online so as to reduce the 



query time [57]. Indeed, using fc-d trees, nearest neighbor 
search can be performed within sublinear time bounds, in the 
worst case [57]. In fact, let us note that the fc-d tree algorithm 
was extended to arbitrary topological manifolds and used for 
nearest neighbor computation in RRTs [58]. 

Worst case logarithmic time, however, is hard to achieve 
in many cases of practical interest. Note that when d — 1, a 
binary tree [59] will allow answering nearest neighbor queries 
in O(logn) time using 0{n) space, in the worst case. When 
d = 2, similar logarithmic time bounds can also be achieved 
by, for instance, computing a Voronoi diagram [60] . However, 
the time complexity of computing Voronoi diagrams is known 
to be 0{n^'^''^^) when d > 2. Several algorithms that achieve 
a worst-case query time that is logarithmic in the number 
of vertices and polynomial in the number of dimensions 
were designed (see, e.g., [61]). However, these algorithms 
use space that is exponential in d, which may be impractical. 
Unfortunately, no exact nearest neighbor algorithm that does 
not scale exponentially with the number of dimensions and 
runs queries in logarithmic time using roughly linear space is 
known [62]. 

Fortunately, computing an "approximate" nearest neighbor 
instead of an exact one is computationally easier In the sequel, 
a vertex y is said to be an e-approximate nearest neighbor 
of a point x if \\y — x\\ < (1 + e) \\z — x\\, where z is the 
true nearest neighbor of x. An approximate nearest neighbor 
can be computed using balanced-box decomposition (BBD) 
trees, which achieves 0{cd,e^ogn) query time using 0{dn) 
space [62], where c^ ^ < d\l + 6d/e\'^. This algorithm is 
computationally optimal in fixed dimensions, since it closely 
matches a lower bound for algorithms that use a tree structure 
stored in roughly linear space [62]. Let us note that using 
approximate nearest neighbor computation in the context of 
both PRMs and RRTs was also discussed very recently [63]. 

Assuming that the Nearest procedure computes an approx- 
imate nearest neighbor using the algorithm given in [62], in 
fixed dimensions the NearestNeighbor algorithm has to run 
in n{\ogn) time as formalized in the following lemma. Let 
^RRT i^g j.jjg random variable that denotes the number of 
steps taken by the RRT algorithm in iteration i. 

Lemma 17 Assuming that Nearest is implemented using the 
algorithm given in [62], which is optimal in fixed dimensions, 
the number of steps executed by the RRT algorithm at each 
iteration is at least order log(A/i) in expectation in the limit, 
i.e., there exists a constant (Prrt G ]R>o such that 

■ _^RRT ^ 



lini inf E 

I— >-oo 



iog(A/;) 



> 



)RRT- 



Likewise, problems similar to that solved by the Near 
procedure are also widely-studied in the literature, generally 
under the name of range search problems, as they have 
many applications in, for instance, computer graphics and 
spatial database systems [57]. In the worst case and in fixed 
dimensions, computing the exact set of vertices that reside 
in a ball of radius r„ centered at a query point x takes 
0{n^^'^/'^ + ra) time using fc-d trees [64], where ra is the 
number of vertices returned by the search. In [65], a thorough 



analysis of the average case performance is also provided 
under certain conditions. 

Similar to the nearest neighbor search, computing approxi- 
mate solutions to the range search problem is computationally 
easier. A range search algorithm is said to be e-approximate 
if it returns all vertices that reside in the ball of size r„ and 
no vertices outside a ball of radius (1 + e) r„, but may or 
may not return the vertices that lie outside the former ball 
and inside the latter ball. Computing e-approximate solutions 
using BBD-trees requires 0(2'' log n + cf^(3v^/£)'^^^) time 
when using 0{dn) space, in the worst case [66]. Thus, in 
fixed dimensions, the complexity of this algorithm is 0(log n-\- 
{i/eY^^), which is known to be optimal, closely matching a 
lower bound [66]. More recently, algorithms that can provide 
trade-offs between time and space were also proposed [67]. 

Note that the Near procedure can be implemented as an 
approximate range search while maintaining the asymptotic 
optimality guarantee. Notice that the expected number of 
vertices returned by the Near procedure also does not change, 
except by a constant factor. Hence, the Near procedure can be 
implemented to run in order logn expected time in the limit 
and linear space in fixed dimensions. 

Let Al?^^ denote the number of steps performed by the 
RRG algorithm in iteration i. Then, together with Lemma 16 
the discussion above implies the following lemma. 

Lemma 18 The number of steps executed by the RRG algo- 
rithm at each iteration is at most order log(A/i) in expectation 
in the limit, i.e., there exists a constant 4>rrg € 1R>0 such that 
-_yy^RRG- 

.log(A/-). 

Finally, by Lemmas 17 and 18, we conclude that the 
RRT and the RRG algorithms have the same asymptotic 
computational complexity as stated in the following theorem. 



Theorem 19 Under the assumptions of Lemmas 17 and 18. 
there exists a constant (f> G M>o such that 

r^RRG- 



lim sup E 



< 



>RRG- 



lim sup E 



M 



RRT 



< 



In section VI, we also provide some experimental evidence 
supporting Theorem 19 

D. On the efficient construction of Probabilistic RoadMaps 

At this point, let us note that our results also imply an effi- 
cient PRM planning algorithm. The PRM algorithm samples a 
set of n vertices from the state-space, and checks connectivity 
of each pair of samples via the Steer and DbstacleFree 
procedures to build a graph (called the roadmap) as follows. 
Any two vertices Xi and X2 are connected by an edge, if 
the path Steer(a:;i,X2) that connects these two samples lies 
within the obstacle-free space'* It is known that this algorithm 

'*In practice, some variations of the PRM algorithm attempt to connect every 
vertex to the vertices that he within a fixed distance. However, this variation 
does not change the theoretical properties such as probabilistic completeness 
or the asymptotic computational complexity. 



is probabilistically complete and, under certain assumptions, 
the probability of failure, if a solution exists, decays to zero ex- 
ponentially. However, notice that, from an asymptotic compu- 
tational complexity point of view, creating this graph requires 
0{n^) calls to the Steer and ObstacleFree procedures. 
On the contrary, our results imply that in a modified version 
of this algorithm, if each vertex is attempted connection to 
vertices within a ball of volume 7l-^^^, then, theoretically 
speaking, probabilistic completeness can be achieved, while 
incurring O(nlogn) expected computational cost. Pursuing 
this direction is outside the scope of this work, and is left to 
future work. 

V. A Tree Version of the RRG Algorithm 

Maintaining a tree structure rather than a graph may be ad- 
vantageous in some applications, due to, for instance, relatively 
easy extensions to motion planning problems with differential 
constraints, or to cope with modeling errors. The RRG algo- 
rithm can also be slightly modified to maintain a tree structure, 
while preserving the asymptotic optimality properties as well 
the computational efficiency. In this section a tree version of 
the RRG algorithm, called RRT*, is introduced and analyzed. 



A. The RRT* Algorithm 

Given two points x,x' E ^frco, recall that Line(a;,a;') : 
[0, s] -^ Xfrcc denotes the path defined by cr(T) — tx + {s — 
t)x' for all T € [0, s], where s — \\x' — x\\. Given a tree 
G — {V, E) and a vertex v E V , let Parent be a function 
that maps v to the unique vertex v' eV such that («', v) E E. 

The RRT* algorithm differs from the RRT and the RRG 
algorithms only in the way that it handles the Extend pro- 
cedure. The body of the RRT* algorithm is presented in 
Algorithm 1 and the Extend procedure for the RRT* is given 
in Algorithm 4 In the description of the RRT* algorithm, the 
cost of the unique path from Xinit to a vertex v eV is, denoted 
by Cost(u). Initially, Cost(a;ijjit) is set to zero. 

Similarly to the RRT and RRG, the RRT* algorithm first 
extends the nearest neighbor towards the sample (Lines 2 3). 
However, it connects the new vertex, x^ow^ to the vertex that 
incurs the minimum accumulated cost up until x^cw and lies 
within the set Xnoar of vertices returned by the Near procedure 
(Lines 6 13). RRT* also extends the new vertex to the vertices 
in Xnoar in order to "rewire" the vertices that can be accessed 
through Xnew with smaller cost (Lines 14 17). 



Algorithm 4: Extend/fi^^T* 


1 V ^V\E' ^ E; 




2 ^nearest ^ Nearest (G, x); 




3 2;now ^ Steer(a;noarost7 2;j; 




4 if ObstacleFree(xnoarcst,a::now) then 




5 


V ^v'u {x„ew}; 




6 


^min ^ ^nearest? 




7 


^ncar ^ Near (G, x„ow, 1^1); 




g 


for all Xnear G ^ncar dO 




9 




if ObstacleFree(xnoar, a^ncw) then 











c' ^ Cost(.i;noar) + c(Line(x„oar , a;now)); 


1 






if c' < Cost(xnow) then 




2 






^min ^ ^ncar? 




3 


£;'^S'U{(x„,i„,a;ncw)}; 




4 


for all Xnear G X^car \ {a^min} dO 




5 




if ObstacleFree(xnow, ajjioar) <^nd 

Cost(a:;noar) > 

Cost(a;now) +c(Line(x„cw,a;„ear)) then 




6 






^parent ^ ^^^®Ilt (Xnear ji 




7 






E' ^ E'\ {(Xparent,a;near)}; 

_ S'^£;'U{(a;„ew,a;ncar)}; 




8 return G' = {V, E') 





From Lemma 20 and Theorem 6 the following theorem, which 
asserts the probabilistic completeness and the exponential de- 
cay of failure probability of the RRT* algorithm, is immediate. 



Theorem 21 If there exists a feasible solution to Prob- 
lem 1 then limi^ooP({Vf^'^* nXgoai 7^0}) = 1. More- 
over, if an attraction sequence A of length k exists, then 
P ({Vfi^T* ^ x^^^i = 0}) < e-^(«P.-2fc)_ 

C. Asymptotic Optimality 

Let yj^^'^ be a random variable that denotes the cost of 
a minimum cost path in the tree maintained by the RRT* 
algorithm, at the end of iteration i. The following theorem 
ensures the asymptotic optimality of the RRT* algorithm. 

Theorem 22 Let Assumptions 12. 13. and 14 hold. Then, the 
cost of the minimum cost path in the RRT* converges to c* 



almost surely, i.e., P ({linii_5.oo 3^, 



RRT' 



'}) = 1- 



B. Convergence to a Feasible Solution 

For all i E N, let Vf-^^' and f^^T* (jgnote the set of 
vertices and the set of edges of the graph maintained by the 
RRT* algorithm, at the end of iteration i. Let V^'^''^ (cj) — 
{xinit} and £^^^' (w) = for all uj E n. 

The following lemma is the equivalent of Lemma 3 



Lemma 20 For all i E N and all uj E ^, V, 

Vf ^G (lo), and £f i^T* (^) ^ £RRG (^)_ 



RRT* 



(CO) 



D. Computational Complexity 

Let A^f^^^ be the number of steps performed by the RRT* 
algorithm in iteration i. The following theorem follows from 
Lemma 20 and Theorem 19 

Theorem 23 Under the assumptions of Theorem 19, there 

exists a constant (p E M>o such that 



lim sup E 



M 



RRT*-] 



M 



RRT 



< 



10 



VI. Simulations 

This section is devoted to an experimental study of the 
algorithms. Three different problem instances are considered 
and the RRT and RRT* algorithms are compared with respect 
to their running time and cost of the solution achieved. Both 
algorithms were implemented in C and run on a computer 
with 2.66 GHz processor and 4GB RAM running the Linux 
operating system. 

We consider three problem instances. In the first two, the 
cost function is the Euclidean path length. The first scenario 
includes no obstacles. Both algorithms are run in a square 
environment. The trees maintained by the algorithms are 
shown in Figure 1 at several stages. The figure illustrates that, 
in this case, the RRT algorithm does not improve the feasible 
solution to converge to an optimum solution. On the other 
hand, running the RRT* algorithm further improves the paths 
in the tree to lower cost ones. The convergence properties of 
the two algorithms are also investigated in Monte-Carlo runs. 
Both algorithms were run for 20,000 iterations 500 times and 
the cost of the best path in the trees were averaged for each 
iteration. The results are shown in Figure 2 which shows that 
in the limit the RRT algorithm has cost very close to a \f2 
factor the optimal solution (see [68] for a similar result in 
a deterministic setting), whereas the RRT* converges to the 
optimal solution. Moreover, the variance over different RRT 
runs approaches 2.5, while that of the RRT* approaches zero. 
Hence, almost all RRT* runs have the property of convergence 
to an optimal solution, as expected. 

In the second scenario, both algorithms are run in an 
environment in presence of obstacles. In Figure 3, the trees 
maintained by the algorithms are shown after 20,000 iterations. 
The tree maintained by the RRT* algorithm is also shown 
in Figure 4 in different stages. It can be observed that the 
RRT* first rapidly explores the state space just like the RRT. 
Moreover, as the number of samples increase, the RRT* 
improves its tree to include paths with smaller cost and 
eventually discovers a path in a different homotopy class, 
which reduces the cost of reaching the target considerably. 
Results of a Monte-Carlo study for this scenario is presented in 
Figure 5 Both algorithms were run alongside up until 20,000 
iterations 500 times and cost of the best path in the trees were 
averaged for each iteration. The figures illustrate that all runs 
of the RRT* algorithm converges to the optimum, whereas 
the RRT algorithm is about 1.5 of the optimal solution on 
average. The high variance in solutions returned by the RRT 
algorithm stems from the fact that there are two different 
homotopy classes of paths that reach the goal. If the RRT 
luckily converges to a path of the homotopy class that contains 
an optimum solution, then the resulting path is relatively closer 
to the optimum than it is on average. If, on the other hand, the 
RRT first explores a path of the second homotopy class, which 
is often the case for this particular scenario, then the solution 
that RRT converges is generally around twice the optimum. 

Finally, in the third scenario, where no obstacles are present, 
the cost function is selected to be the line integral of a function, 
which evaluates to 2 in the high cost region, 1/2 in the low 
cost region, and 1 everywhere else. The tree maintained by the 



RRT* algorithm is shown after 20,000 iterations in Figure 6 
Notice that the tree either avoids the high cost region or 
crosses it quickly, and vice-versa for the low-cost region. 
(Incidentally, this behavior corresponds to the well known 
Snell-Descartes law for refraction of light, see [69] for a path- 
planning application.) 

To compare the running time, both algorithms were run 
alongside in an environment with no obstacles up until one 
million iterations. Figure 7, shows the ratio of the running 
time of RRT* and that of RRT versus the number of iterations 
averaged over 50 runs. As expected from the complexity 
analysis of Section IV-C, this ratio converges to a constant 
value. The same figure is produced for the second scenario 
and provided in Figure 8 
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Fig. 2. The cost of the be.st paths in the RRT (shown in red) and the RRT* 
(shown in blue) plotted against iterations averaged over 500 trials in (a). The 
optimal cost is shown in black. The variance of the trials is shown in (b). 



VII. Conclusion 

This paper presented the results of a thorough analysis of 
the RRT and RRG algorithms for optimal motion planning. It 
is shown that, as the number of samples increases, the RRT 
algorithm converges to a sub-optimal solution almost surely. 
On the other hand, it is proven that the RRG algorithm has the 
asymptotic optimality property, i.e., almost sure convergence 
to an optimum solution, which the RRT algorithm lacked. The 
paper also proposed a novel algorithm called the RRT*, which 
inherits the asymptotic optimality property of the RRG, while 
maintaining a tree structure rather than a graph. The RRG and 
the RRT* were shown to have no significant overhead when 
compared to the RRT algorithm in terms of asymptotic com- 
putational complexity. Experimental evidence that demonstrate 
the effectiveness of the algorithms proposed and support the 
theoretical claims were also provided. 

The results reported in this paper can be extended in a 
number of directions, and applied to other sampling-based 
algorithms other than RRT. First of all, the proposed approach, 
building on the theory of random graphs to adjust the length 
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Fig. 1. A Comparison of the RRT* and RRT algorithms on a simulation example with no obstacles. Both algorithms were run with the same sample 
sequence. Consequently, in this case, the vertices of the trees at a given iteration number are the same for both of the algorithms; only the edges differ The 
edges formed by the RRT* algorithm are shown in (a)-(d) and (j), whereas those formed by the RRT algorithin are shown in (e)-(h) and (i). The tree snapshots 
(a), (e) contain 250 vertices, (b), (f) 500 vertices, (c), (g) 2500 vertices, (d), (h) 10,000 vertices and (i), (j) 20,000 vertices. The goal regions are shown in 
magenta (in upper right). The best paths that reach the target in all the trees are highlighted with red. 



of new connections can enhance the computational efficiency as kino-dynamic planning problems. A third direction is the 

of PRM-based algorithms. Second, the algorithms and the optimal planning problem in the presence of temporal/logic 

analysis should be modified to address motion planning prob- constraints on the trajectories, e.g., expressed using formal 

lems in the presence of differential constraints, also known specification languages such as Linear Temporal Logic, or 
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Fig. 3. A Comparison of tlie RRT (shown in (a)) and RRT* (shown in (b)) algorithms on a simulation example with obstacles. Both algorithms were ran 
with the same sample sequence for 20,000 samples. The cost of best path in the RRT and the RRG were 21.02 and 14.51, respectively. 



Finally, it is noted that the proposed algorithms may have 
applications outside of the robotic motion planning domain. 
In fact, the class of incremental sampling algorithm described 
in this paper can be readily extended to deal with problems 
described by partial differential equations, such as the eikonal 
equation and the Hamilton-Jacobi-Bellman equation. 



(a) 
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18000 20000 



(b) 

Fig. 5. An environment cluttered with obstacles is considered. The cost 
of the best paths in the RRT (shown in red) and the RRT* (shown in blue) 
plotted against iterations averaged over 500 trials in (a). The optimal cost is 
shown in black. The variance of the trials is shown in (b). 



the ^-calculus. Such constraints correspond to, e.g., rules of 
the road constraints for autonomous ground vehicles, mission 
specifications for autonomous robots, and rules of engagement 
in military applications. Ultimately, incremental sampling- 
based algorithms with asymptotic optimality properties may 
provide the basic elements for the on-line solution of differ- 
ential games, as those arising when planning in the presence 
of dynamic obstacles. 
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Appendix 

Proof of Theorem 10 

Since the feasibility problem is assumed to admit a solution, 
the cost of the optimal path, c*, is a finite real number. First, 
consider the following technical lemma. 

Lemma 24 The probability that the RRT constructs an opti- 
mal path at a finite iteration z e N w zero, i.e., 

p(u.eN{:yr^-c*})=o. 

Proof: Let Bi denote the event the RRT has a path 
that has cost exactly equal to c* at the end of iteration i. 



i.e., B, = {y.^ 



_ r^?RRT _ 



^}. Let B denote the event that the 



RRT finds a path that costs exactly c* at some finite iteration 
i. Then, B can be written ?& B ~ Ui^mBi. Notice that 
Bt C B,+i; thus, we have that limj^ooP(S*) = P(S), by 
monotonocity of measures. Notice also that by Assumptions 7 
and 8, V{Bi) = for all i e N, since the probability that 
the set y*- j^{Sample(j)} of points contains a point from a 
zero-measure set is zero. Hence, ¥{B) ~ 0. ■ 

Let Ci denote the event that Xinit is chosen to be extended 
by the Nearest procedure at iteration i. The following lemma 



direcly implies a necessary condition for asymptotic optimal- 
ity: with probability one, the initial state, Xjnit, must be chosen 
for extension infinitely often. 



Lemma 25 The following inequality holds: 

P (\ lim yf-^^ = c*|) < P ( limsupC, 



Proof: Denote by ip{x) a minimum-cost path starting 
from a given state x E Xfroc and reaching one of the states 
in the goal region^, and define a sequence {cijigpf of random 
variables as follows: 

Ci = min |Cost(Line(a;ijjit, x) \ tl:{x)) 

xevr'',ix,^,„x)e£r^}. 

Essentially, Ci denotes the minimum cost that is incurred by 
following the line segment that connects Xinit to one of its 
children and following an optimal path afterwards. Notice that 
Ci is strictly greater than the optimal cost c* for all i E N, 
unless the root node, Xinit, has a child node that is a part of 
an optimal path. 

Recall, from the proof of Lemma 24, that B denotes the 
event UigpflX^^'^ — '^*}- Let Ai denote the event that q de- 
creases at iteration i, i.e., Ai — {ci < Q^i}. First, notice that, 
conditioning on the event B"^, we have that the convergence 
of the cost of the best path in the RRT to the optimum cost 
(i.e., the event {limi^oo 3^^^^''" = c*}) implies that Ci must 
decrease infinitely often, i.e., Ai must hold infinitely often. 
More precisely, we have that {limj^oo X™'^ — c*} f] B'^ C 
limsupj^^ Aj. 

Noting that V{B'') = 1 (by Lemma 24), it follows that 

F (^[ikn^y^''^ = c*}) = V (^[l\m^y^''^ = c*} n B'^) 



< 



lim sup A, 



Notice that, by Assumption 9, for Ai to occur 
infinitely often, Ci must also occur infinitely 
often, i.e., limsup^^^ Ai C limsupj^^^ C^. Thus, 
V{\iinsupi^^)Ai < P(limsupj^^ Ci), which implies 
the lemma. ■ 

To prove the theorem, it is shown that RRT fails to satisfy 
this necessary condition: 

Lemma 26 The probability that the RRT algorithm extends 
its root node infinitely many times is zero, i.e.. 



lim sup Ci] = 0. 



Proof: Let {Ci,C2, . . . ,Ck} be a finite set of cones, 
such that (i) each cone is centered at Xinit and has aperture 
at most 7r/3, and (ii) collectively the cones cover the ball 
of radius -q around Xjnit- Defined for all k E {1,2, .. . ,K} 
and all j E N, let Dk,j be the event that a node from inside 
cone Ck is connected to Xinit for the jth time. Clearly, for 

'if there is no such path, then let A be a symbol for "unfeasible path," and 

set ■tp{x) = A, with Cost(A) = -f oo. 
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Ci to occur infinitely often, Dk.j must occur for infinitely 
many j, for at least one k. More formally, limsupj_^3^ Ci C 



^k=i limsupj 



*oo -Dfc J, 



and that 



limsupCi) < 



f^ilimsupIJfcj] 



K 



< ^P(limsupDA;,^ 



fc=i 



= 



The next step is to show that F{liinsupj^^ Dkj) 
holds for all fc G {1, 2, . . . , K}, which implies the lemma. Let 
k be any index from {1,2,..., K}. Let Vkj denote the set of 
vertices inside cone Ck right after the connection of jth node 
to a;init from inside the cone Ck- Let r^in,k.j be the distance 
of the node that is inside Vk.j and has minimum distance to 
Xinit among all the vertices in Vk.j, i.e.. 



^min,fc,j 



mm \\v - 



a;init||- 



This distance is a random variable for fixed values of k and 
j. Let /r„i„,fc,j denote its probability density function. Also, 
given two points x,y ^ X, let "D^y denote the set of all points 
in X that are closer to y than they are to x, i.e.. 



V 



x,V 



{zeX\ \\x^z\\ > \\y-z\\}. 



For any j e N>o, conditioning on rmin.fcj = z, the 
probability that a node from Ck is connected to Xj^n for 
the jth time is upper bounded by the measure of the region 
Ck\T^xi„it,y, where y is any point with distance z to Xinit (see 
Figure 9). Hence, F{Dk,j \rj^in,k,j = z) < a^{Ck \ 'Dxi^,,^y) 
for some contant a S M>o by Assumption 8 Furthermore, 
since Ck has aperture at most 7r/3, the region Ck \ T^xi„it,y 
is included within the ball of radius z centered at Xjnit (also 
illustrated in Figure 9). Hence, 



nDk,j] 



< 



kj I ^min,fc,j — ^ jJrmin.kj \Z)(IZ 



a^i{Ck \ V^^^^^^y)fr^ Az)dz 



z=0 



'z=0 

Note that |l{Bx^.^^^.z) < Cdz"^, where d is, recall, the dimen- 
sionality of the smallest Euclidean space containing X, and 
C,d is the volume of the unit ball in this space. Hence, 



k,]) <aCd z'^fr,^.^^^Jz)dz = aCd E[(rmi„,fcj)'*], 



where the last equality follows merely by the definition of 
expectation. By the order statistics of the minimum distance, 
we have that E[{r-^ink,jY] evaluates to -^ for some constant 
/3 e M>o, under Assumption 8 (see, e.g., [70]). Hence, we 
have that, for all k, J27Li^{Dk,j) < oo, since d > 2. Then, 
by the Borel-Cantelli Lemma [71], one can conclude that the 
probability that Dkj occurs for infinitely many j is zero, i.e., 
P(limsup _j.3j3 Dk,j) — 0, which implies the claim. ■ 

The theorem is immediate from Lemmas 25 and 26 




Fig. 



Illustration of intersection of Cfe (shaded in blue) of aperture i/i at 



least 7r/3 with the region D^ 



. The ball B^ 



of radius z that includes 



Ck \ »Xi, 



is also shown with dotted lines in the figure. 



ball with volume 7- 
we have r^ = ( p- -^ 



Proof of Theorem 15 

In order to prove that 3^^?^*-^ converges to c* almost surely, it 
will be shown that YT^o f{yf^^ > c*+s) is finite for all e > 
(see, for instance, [71] for justification of this implication). 

Recall that the volume of the unit ball in d dimensions is 
denoted by Q- Recall also that r^ denotes the radius of the 
- used in the Near procedure and that 

l/d 

. Let (7* : [0, s*] -)■ cl(Xf,.oo) be an 
optimal path with length s* . Recall that i denotes the iteration 
number (see Algorithm 1). The proof is first outlined in the 
following section, and then described in detail. 

Outline of the proof: First, construct a sequence {XijigN 
of subsets of Xfioo such that (i) the sequence is monotonically 
non-decreasing in the partial subset ordering, i.e., Xi C X^+i 
for all i e N, (ii) for all large enough i e N, any point in 
Xi is at least a certain fraction of r^ away from obstacles, 
i.e., \\x ~ y\\ > Xi for all x G Xi and all y £ Xobs. where 
\i — ari for some constant a (see Figures 10(a) and 10(b)), in 
particular, the set Xi is included in Xfree, and (iii) the sequence 
converges to Xfroo in the sense that UigN^i = ^free- 
Second, construct a sequence {(TijigN of paths such that (i) 
for all i e N, the path Ci lies completely inside Xi, and (ii) 
the sequence of paths converges to the optimal path a*, i.e., 
limi_^oo ll^i — (7*11 = 0. This can be done by dividing a* into 
segments and constructing an approximation to any segment 
that lies outside Xi (see Figure 10(c)). Note that, since for all 
large enough i, any point in Xi is at least a certain fraction of 
Tj away from the obstacles, in particular, any point on the path 
ai is at least a certain fraction of r^ away from the obstacles, 
for all such i e N. 

Third, for any i E N, construct a set Bi of overlapping 
balls, each with radius qi and centered at a point on the path 
ai, such that the balls in Bi collectively "cover" the path ai 
(see Figure 10(d)). Moreover, the radius Qi is chosen in such 
a way that for all large enough i, we have that qi is less than 
ri. Hence, for all such large enough i, all the balls in Bi 
completely lie inside the obstacle-free region. 

Finally, compute the probability of the event that at least 
one ball in Bi contains no node of the RRG in iteration i; this 
event is denoted by Ai. Subsequently, one can show that Ai 
can not occur infinitely often as i approaches infinity, which 
implies that its complement, A'l, must occur infinitely often 
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(with probability one). That is, one can conclude that the RRG 
will include a node in each ball in the set Bi of balls for 
infinitely many i's, with probability one. Joining the vertices 
in subsequent balls in Bi, one can generate a path, whose 
cost is shown to be close to c*. Moreover, as i approaches 
infinity, the costs of such paths converge c*. Appropriately 
chosing Qi guarantees that the RRG algorithm joins the points 
in subsequent balls by an edge, i.e., the path is included in the 
RRG. Hence, the result follows. 

The assumptions on the cost function are mostly used to 
make the convergence arguments possible. The assumptions 
on the obstacles (i.e., environment), on the other hand, are 
primarily used for ensuring that the paths in the sequence 
{CTi}igN are collision-free. 

The proof technique is based on the bin-covering technique 
[72], which is widely used for analyzing almost-sure prop- 
erties of random geometric graphs (see, e.g., [73]). Similar 
methods were also used for analyzing PRMs (see [33] and the 
references therein). 




(a) 



(b) 



(c) 





(e) 



Fig. 10. A region witli obstacles is illustrated in Figure 10(a) An optimal path 
is also shown and denoted as <j*. In Figure 10(b) the region that lies outside 
the light orange shaded shaded area (enclosed by the dotted lines) illustrates 
the set Xi. Notice that the dotted lines, hence all the points in the set Xi, 
have distance at least A^ to any point in the obstacle region. In Figure 10(c) 
the optimal path is divided into parts and the segments Pi , P2 1 • ■ • i Ps are 
constructed so that pi , pa , and ps (solid lines) lie inside Xi , whereas p2 and 
P4 (dotted lines) lie inside Xf^ee \ ^i ■ Note that these path segments together 
make up the path ai, i.e., ai = pi|p2| ■ . ■ Ips- In Figure 10(d) the tiling of 
(Ji with overlapping balls in Bi are illustrated. Finally, in Figure 10(e) this 
tiling is shown in detail. 



Construction of the sequence {Xi^n^^: Let 6i G (0, 1) be a 
constant. First, define a sequence {AijigN of real numbers as 
Ai = min \ S, |xf^^i k where 6 is given by Assumption 14 
Construct a sequence {Xijigpj of subsets of Xfj-ee as follows 
(see Figure 10(b)): 

Xi ;= Xfroc \ {a; € ^frcc|3x' € Xobs-lja; - x'\\ < AJ. 



Denote the boundary of Xi by dXi. The precise value of the 
constant di will become clear shortly. 

Construction of the sequence {ai}it^^: Assume, without 
loss of any generality, that the beginning and the end of the 
optimal path a* are at least 5 away from the obstacles, i.e., 
for r e {0, s*}, it is the case that \\'7*{t) — x\\ > 5 for all 

X e Xobs- 

Construct a sequence {(Ti}i£N of paths, where ai : [0, Si] —>■ 
Xi, as follows (see Figure 10(c) for an illustration). First, 
break up the optimal path a* into a minimum number of 
segments such that each segment completely lies either inside 
Xi or inside cl(Xfi-co)\-'i^i- For this purpose, define a sequence 
{tq, ti, . . . , Tm+i} of scalars as follows: 

• To = and t„j+i = s*. 



for all j' e {1, 2, . . . , m}, Tj e dXi, and 



T e 



. for all {0,1,..., to}, either cr*(r') e X, for all 
{tj,Tj+i) or cr*(r') ^ X^ for all r G {tj,tj+i). 
Second, for each segment of <t* defined by its end points 
cr*(Tj) and i7*(tj+i), construct a path that lies in Xi and 
agrees with this segment of a* in its endpoints. More precisely, 
define a set {po, pi, . . . ,pm} of paths as follows: (i) if 
cr*(T') G Xi for r' G {tj, Tj+i), then let pj be the jth segment 
of a*, i.e., Pj{t' — r,) = cr*(r') for all t' G [tj,tj+i], (ii) 
if, on the other hand, <t*{t') ^ Xi for all r' G (rj,Tj+i), 
then let pi be a minimum-cost continuous path in Xi starting 
from (7*(tj) and ending in (t*(tj+i). Such apath always exists 
by the construction of Xi and Assumption 14 Finally, define 
ai as the concatenation of path segments pi, p2, ■ ■ ■ , Pm, i-e., 
define ai := pq\pi\ ... |p„i- 

Two important properties of the sequence of paths {crijigN 
follow. First, each path ai is at least A, away from the 
obstacles. More precisely ||o'i(r) — x\\ > Xi for alH G N, t G 
[0, Si], and x G ^obs- This claim follows from Assumption 14 
and the fact that Xi < 6 for all i. The second property, 
regarding the costs of the paths in {cTijigN, is stated in the 
following lemma: 



Lemma 27 Under Assumptions 12 and 13. lim^ 
c{a*) holds. 



, c(cri) 



Proof: First, for all i G N, 0-^ is continuous by construc- 
tion. Second, note that Xi C Xj+i for all i E N, and that 
UigN-'^i = -'^frco- Let nii be the number of segments of a* 
that lie in Xfrcc \ Xi. By Assumption 13, the cost of each 
such segment is bounded by kA^. Then, by Assumption 12 
\c{ai) — c{a*)\ < iriiKXi. Finally, since rrii is a monotonically 
non-increasing function of i, and Ai converges to zero as i 
approaches infinity, the lemma follows. ■ 

Construction of the sequence {Bi}i^fq of sets of balls: Next, 
for each i G N, construct a set Bi of equal-radius overlapping 
balls that collectively "cover" the path ai. 

Recall the constant di G (0, 1), introduced above. The 
amount of overlap between two consecutive balls will be 
parametrized by this constant di. Denote the radius of each 

for reasons 



Ai 



(1+ei) 



ball in Bi by qi, which we define as q 
to become clear shortly. 

Construct the set Bi ~ {S; i, -6^,2, • ■ ■ , Sj A-.+i} of balls 
recursively as follows (see Figures 10(d) and 10(e)): 
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• Let Bii be centered at CTi(O). 

• For all k > 1, let Bi^k centered at CTi(T), where t is such 
that the centers of Bi ^ and Bik_i are exactly 6i qi apart 
from each other. 

• Let Ki be the number of balls that can possibly be 
generated in this manner and, finally, let Bi^K i+i be the 
ball centered at ai{si). 

Now, consider paths that can be constructed by con- 
necting points from balls in Bi in a certain way. More 
precisely, for all k E {1,2, ... ,Ki + 1}, let Xk be 
a point from the ball Bi^. Consider a path a'^ that 
is constructed by joining the line segments connecting 
two consecutive points in {xi,X2, . . ■ ,XKi+i}, i-C-, o-'i = 
L±Tie(xi,X2)\L±Tie(x2,X3)\ . . .\L±ne{xKi,XKi+i)- With an 
abuse of notation, let E^ denote the set of all such paths. 

First, notice that, by the choice of the radius qi of the balls 
in Bi, any path a'^ E S^ is collision-free. 

Lemma 28 Let Assumption 14 hold. For any path a[ G SJ, 
where a[ : [0, sj] -^ X, a[{T) G Xfroc for all t e [0, sj]. 

Proof: Let {xi, X2, . . . , xni+i] be the set of points that 
forms (j[ = Liiie{xi,X2)\ . . .\Line{xKi,XKi+i)- It will be 
shown that each segment Line(xfe, Xk+i) is obstacle-free, i.e., 
lies in Xfroc, which implies the lemma. Let yk denote the 
center of the ball Bij^. Note that Xk and yk have distance at 
most qi. Similarly, noting that Xk+i has distance at most qi 
to yk+i and that y^ and yk+i are at most di qi apart from 
each other, one can conclude that Xk+i has distance at most 
{l + 9i)qi to yk. Indeed, yk has distance at most {l + 6i)qi = 
Xi to any point in the convex combination [xk,Xk+i]. Finally, 
recall that, as a consequence of Assumption 14, any point with 
distance at most A^ from yk lies in Xfrco^ which completes the 
proof ■ 

Second, notice that the cost of cr^ is "close" to that of (7^. 
Indeed, the following lemma holds. 

Lemma 29 Let Assumptions 12 and 13 hold. Let {cr^}ieN be 
any sequence of paths such that (j[ G SJ for all i G N, then 
lim^^co |c(cr-) - c(o-*)| = 0. 

Proof: The distance between a path ct^ G S^ and the 
path ai approaches zero as i approaches infinity. Moreover, 
by Assumptions 12 and 13, \c{a[) — c{ai)\ converges to zero. 
Hence, the result follows from Lemma 27 ■ 

Let us define Si — sup^./^^;/ \c{a[) — c*|. Hence, Lemma 29 
establishes that limi^oo £i = 0. 

Third, according to our choice of qi, any path in EJ is 
"constructable" by the RRG in the following sense. 

Lemma 30 For all i E N and for all k G {1,2, ... ,Ki}, 

\\xk+i - Xk\\ < n, where Xk G B^^k- 

Proof: Recall that the radius of each ball in Bi is 
Qi = (1+0 ) ■ Given any two points Xk and Xk+i, both Xk 
and Xk+i have distances at most qi to the centers of the balls 
Bi^k and Bi^k+i, respectively. Note also that the centers of 
these balls have distance at most 9i qi to each other. Using 



the triangle inequality together with the definitions of A^ and 



qi, one obtains ||a:;fc+i — a;fc|| < (2 + Oi)q, 



_ 2+01 
1+8] 



A,: < 



Intuitively, Lemma 30 establishes that if each ball in Bi 
contains at least one node of the RRG by the end of iteration 
i, then the RRG algorithm will indeed connect these vertices 
with edges and construct a path cr^ from EJ. 

The probability that a path from EJ is constructed: Let Ai,k 
be the event that the RRG has no node inside the ball Bik 
at the end of iteration i. Moreover, let Ai be the event that at 
least one of the balls in Bi contains no node of the RRG at 
the end of iteration i, i.e., Ai = Ufc=i ^i,k- 

It is appropriate to explain how the main result of this 
theorem is related to the sequence {^ijigpf of events. Recall 
that Ei was defined as Si — supg./gjn/ \c{<Ti)—c* \ and that it was 
already established that e^ converges to zero as i approaches 
infinity. Hence, given any e, one can find a number zi G N 
such that for all i > ii, Si < e. Recall also that r^ converges to 
zero as i approaches infinity. Hence, there exists some J2 G N 
such that ri < S for all i > 12. Let ig = maxjii, 12] and note 
the following upper bound: 



Y^v{{yr'^>c*+e]) 



«^0 

IQ — 1 00 

= E P ({ J^f «« > c* + £}) + ^ p {{yf^^ >c* + s]) 

i—0 i—io 

00 00 

< zo + E P ({J^f^« > c* +£,})< zo + E 'P(^^)- 

i=ia i=iQ 

To complete the proof, it will be established that 
Y.T=o^i^i) < oo' which implies that E^o^d^^^ > 
c* + e}) < 00 for all e > 0, which in turn implies the result. 

To formally show this claim, define the following sequence 
of events. For all i G N, let Ci be the event that for any point 
X G ^froo the RRG algorithm includes a node v such that 
\\x — v\\ < -q and that the line segment Line(x,w) joining v 
and X is obstacle-free. Under the assumptions of Theorem 6 
the following lemma holds: 

Lemma 31 Let the assumptions of Theorem 6 and Assump- 
tion 14 hold, then there exist constants a,b E M>o such that 
P(Cf ) < ae-'" for all i G N. 

Proof: The set Xfroe can be partitioned into finitely 
many convex sets such that each partition is bounded by a 
ball of radius 77, since Xfrcc is a bounded subset of R''. Since, 
by Theorem 6 the probability that each such ball does not 
include a node of the RRG decays to zero exponentially as 
i approaches infinity, the probability that at least one such 
ball does not contain a node of the RRG also decays to zero 
exponentially (this follows from the union bound). Moreover, 
if each partition contains a node of the RRG, then any point 
inside the partition can be connected to the associated node 
of the RRG with an obstacle-free line segment of length less 
than 7], since each partition is convex and is bounded by a ball 
of radius 1]. ■ 

The following lemma constitutes another important step in 
proving the main result of this theorem. 
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Lemma 32 Let 7 > 2''(1 + l/d)^(Xfrco), fln<i O2 e (0, 1) fee 
a constant. Then, 'Yl^i^i-^i 01=161. i\ ^i) "^ °°- 

Proof: It is desired to compute V{Ai \ n* 



= L02»J 



^ I Cj ) under 



Proof: Note the following inequalities: 

00 00 



the theorem's assumptions. Since C?-ig ^iCj is given, it can 
be assumed that, from iteration [612 i\ to iteration i, the RRG 
has the following coverage property: for every point x e Xfrco, 
the RRG includes a node v that is at most rj away from x and 
the line segment between x and v lies in the obstacle-free 
region. 

Recall that the length of ai was s^. A bound on the number 
of balls in Bi with respect to i, Si, and other constants of the 
problem can be derived as follows. Note that the segment of di 
that starts at the center of Bi^k and ends at the center of -Bi.fe+i 
has length at least 61 qi (recall that distance between the 
centers of two consequtive balls in Bi is OiQi by construction), 
except for the last segment, which has length less than BiQi- 
Thus, for all i > 12, 



00 ^ 



00 X 



<E E p(q),<E E «'^-'^ 

in which the right-hand side is finite for all 62 G (0,1), and 
where the second inequality follows from Lemma 31 ■ 

Finally, the relationship of Lemmas 32 and 33 to the main 
result can be pointed out. Note the following inequality: 






'j=y02i\^]> 



\B, 



K. 



1 < 



(2 + 0i)s, 



2 + 1 






1/d 



logi 



l/d 



= i-p(4=u(n}^L«2.j^.)j 
>i-p(A^)-p((n}^L«^,jQr) 

= p(A,)-p((n}^L«,,jC,r 



Also, given n}^Le2»J 



C. 



yj, the probability that a single ball, 
say Bii, does not contain a node of the RRG at the end of 
iteration i can be upper-bounded as follows: 



P(Ai|n}^L«^,jQ) 



< 



1 



m(s». 



^(Xfrcc) 

7 



logi 



{2 + elY^l{Xi,,,) I 



{1-62) i 



Hence, taking infinite sums of both sides with respect to n 
and rearranging, the following inequality is established: 

00 00 00 

E p(^o < E ip(^^ I '^l^L^^ .J ^.o + E IP (('^^-^L^^ .J c.) 

By Lemmas 32 and 33 the right-hand side is finite, whenever 

7 > 2'^(1 + l/d)iJ,{Xf^cc), which implies that, under the same 
conditions, YlTLi IP(^i) < co, which in turn implies the result. 



Using the fact that (1 - llf{i)Y < e~''//(*), the right-hand 
side can further be bounded to obtain the following inequality: 

logi 



As a result of the discussion above, we have that 



Proof of Lemma 16 

The proof of this lemma employs results from the Random 

Geometric Graph (RGG) theory (see, e.g., [53]). First, some of 

^(^i,! I ^t=\e2i\ Q) ^ e"(2+<'i)''^'<^froo) '"*"' = j"(2+«i)''M(Xf^eo) thc rclcvant results in the theory of RGGs will be introduced, 

and then the proof of this lemma will be given. 

An RGG G(]4,,r„) = (y„,£'„) in Xf^^c with n vertices 
is formed as follows: (i) Vn is a set of n vertices that are 
sampled identically and independently from Xfrcc according 
to a distribution with a continuous density function f{x), (ii) 
two vertices u, u' G Vn are connected with an edge if and only 
if the distance between them is less than r„, i.e., ||i;— w'|| < r„. 

The following lemma is a special case of Proposition 3.1 
in [53]. 



AA ^)=v0-2t\ ^3 



< p(uSU 



n}=Le2*J ^J 



< 



< 



El 

i=l 



A.. 



n}=Le2jJ ^J' - l-^'- 



2s,Ai(^frcc)l/'' / * 



yl/d 



2s,Ai(^frcc)l/'' 



logi 

1 



l/d 



A 



{l~f>2)"l 



^]=[02H '^3 



I (2 + ei)''M(Xf,^^) 



71/d (logi)!/^ 

NotethatE^iP(^.| n}^|, 
(1-^2)7 



(l-f2)-f 

n(Xf„„)(2+ei)'i 



ji Cj) < cx), for all choices 
of 7 such that ,J"''~ZV7a ^rf ~ ^ > 1 holds, i.e., whenever 
7 > 2'^(1 + l/(i)/j.(Xfroo), noting that for any such choice 
of 7, there exists 9i > and 62 > satisfying the former 
inequality. Hence, the lemma follows. ■ 



Lemma 34 (see [53]) If r„ satisfies lim„^oo ^n = 0, then 

n\E.\]_K f ^(,... 



1™ 2 f \d 



^Cd 



fixfdx. 



Lemma 33 Let 62 

E^iP((n}^ 



[02 r] ^3 



G (0, 1) be a constant. Then, 
< 00. 



Note that, if f{x) is the uniform density function, then 

L(zRd f{xf dx = l/^(Xfroc). 

Let V° denote the number of vertices that are isolated, i.e., 
those that do not have any neighbors. The following lemma 
characterizes the "density" of the isolated vertices in the limit. 
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Lemma 35 If C,d{rnY =■ 7^^ for some 7 e M>o, then the 
number of isolated vertices, \V°\, satisfies the following: 

n-l 



nv°\] 









n 


Thus, lim„_ 
whenever 7 


-s-oc 

> 


nv:\] 

n 

1. 



< 1-7 



logn 



^ ^^00 E[|y„°|] . ^ . 

= (J; moreover, > „ , " ii nnUe 



Proof: Reference [53] does not consider the case when 
n{rnY ^ 00 as n ^ 00. However, following the reasoning in 
the proof for the case when n{rnY ^ P '= If^>o (see the proof 
of Proposition 3.3 in [53]), it is possible construct a proof for 
this lemma. Let us sketch this proof. 

Following [53] (note that the the notation in [53] is slightly 
different from that in this paper), and after appropriate sim- 
plifications, one gets 



(l-/„(x))" f{x)dx, 



XeXfra 



where f{x) is the density function, which is uniform 
over Xfree in the case under consideration, and In{x) — 
f ,,K x'dx', which evaluates to /„ ~ ^-2221 whenever 

(^ri)'' ~ 7-^ (the reader is referred to [53] for details). Hence, 



nv°] 



a: entree 



■7- 



logn 



//(Xfrco) 



-dx 



1 



7- 



logn 



which converges to zero as n approaches infinity. Moreover, 
the sum of these terms is finite whenever 7 > L ■ 

Consider the random geometric graph with the vertex set 
Vi that is formed with the Sample procedure, i.e., Vi = 
U*-^]^{Sainple(j)}. Let V^ denote the set of isolated vertices 
and let V/ denote the set of the remaining vertices, i.e., 
V^ ^ V^\ VlK We will denote by ^0 and E\ the set of 
all edges that connect the vertices in V^ and those in V^ , 
respectively. Hence, we have that E^ = and Ej = En for 
all i e N. 

Consider the RRG algorithm. Let V^^ denote the set of 
all vertices that result from extending the graph when the 
corresponding sample is isolated, i.e., no vertices were present 
within the ball of volume 7 °fr' centered at the sample, 
Sample(i), in iteration i. Let Ef denote the set of all edges 
that are connected to the vertices in V^^. 

Notice the following two facts: (i) the random geometric 
graph with node set Vi can be described by Grog,! ~ 
(VP U Vj'-^E" U E}), and (ii) the graph maintained by the 
RRG algorithm is a subgraph of Grrg i = (Vf ^°, ff ^°) = 
{V,^UV,\EluEf). 



Notice that the number of calls to ObstacleFree in itera- 
tion i is exactly the number of edges created at that iteration, 
hence is less than or equal to \E} U Ef\ = \El\ + \Ef\. The 
following lemmas lead immediately to the result: 



Lemma 36 



lim sup E 



\E} 



A/;iog(M) 



1 
< - 



7 



2 ^{Xfj-cc) 



Proof: Consider the expected value of \El\/{n'^ (rnY), 
when n — Afi- Substituting Cd(''n)'^ = 7 °^. '^ and using 
Lemma 34, the result follows. ■ 



Lemma 37 limsupj 



,E 






is finite. 



Proof: Let M? denote the number of edges added to Ef 
at iteration i. Note that 



E[Mf ] < E 



log(M) 



7 



lJ-{Xhcc) 

Hence, noting Ef = Yl\=i ^i- 
\E,\ 



l^iXircc) 



E 



log(M) | 2| 



lim sup E 



Miog(M) 



< lim sup E 



l°g(A/"j) |T^2| 

Miog(M) 



2^1=1 



Consider the expectation in the right hand side. Noting that, 
surely, \og{Mj) < log(A/i) for all j < i, the following 
inequalities hold: 



E 



Miog(M) 



< E 



< E 



< E 




where the second inequality follows from the Cauchy-Schwarz 
inequality. Notice that the first term approaches zero as i — > 
00, whereas, by Lemma 35 , the second term is finite. Hence, 
it is concluded that 



1 

Wi 



-,1/2 



lim sup E 
which implies the lemma. 




Af, 
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< 00, 



